#!/usr/bin/env python
import sys
import random
import roslib; roslib.load_manifest('object_seeking')
import rospy
from geometry_msgs.msg import Twist
from irobot_create_2_1.msg import SensorPacket
from cmvision.msg import Blobs
from cmvision.msg import Blob
from ar_recog.msg import Tags

i = 0 
seekOrder = None
imageWidth = 640
targetTag = None
targetBlob = None
firstIter = None
targetReached = False
targetObj = None
lastError = 0.0
speed = .20
maxError = 10

#alpha tag beta tag
targetArray = [(False, 0,0,215),(False, 1, 1, 215)]

def processSensing(sensorPacket):
    global bump
    bump = sensorPacket.bumpLeft or sensorPacket.bumpRight

def movement():

    global speed
    global lastError
    global targetTag
    global targetReached
    global maxError
    global imageWidth
    global firstIter
    global targetBlob

    twist = Twist()

    twist.linear.x = 0; twist.linear.y = 0 twist.linear.z = 0
    twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0

    if targetBlob is not None:
        newError = targetBlob.x - (imageWidth * 0.5)

        if abs(newError) < maxError:
            newError = 0
        
        if firstIter:
            lastError = newError;
        
        twistSpeed = -(newError - .5*(lastError - newError))/(imageWidth*.5)

        twistSpeed = min(max(twistSpeed,-1),1)
        
        twist.angular.z = twistSpeed
        twist.linear.x = speed * (1.0 - abs(twistSpeed))

        lastError = newError

        firstIter = False

    elif targetBlob is None and not targetReached:
        twist.angular.z = 0.5
        firstIter = True
    
    else:
        return twist

    
def processTags(tags):
    
    global targetObj
    global targetBlob
    global targetReached
    global imageWidth

    if targetObj is None or targetObj[0]:
        return

    imageWidth = tags.image_width
    targetTag = None
    theSize = 0
    maxSize = 0
    
    if tags.tag_count > 0:
        for tag in tags.tags:
            sideX = abs(tag.cwCorners[0]-tag.cwCorners[4])
            sideY = abs(tag.cwCorners[1]-tag.cwCorners[5])
        
            theSize = max(sideX, sideY)

            if tag.id is targetObj[1] and theSize > maxSize:
                maxSize = theSize
                targetTag =tag

    elif maxSize > targetObj[3]:
        targetReached = True
    targertBlob = targetTag

    else:
        print "You have no tags"
    

def hello_create():
    pub = rospy.Publisher('cmd_vel', Twist)
    rospy.Subscriber('tags', Tags, processTags)
    rospy.Subscriber('sensorPacket', SensorPacket, processSensing)
    rospy.init_node('hello_create')

    global targetBlob
    global targetObject
    global targetReached
    global seekorder

    seekorder = rospy.get_param('seekorder')

    targetObj = getNext()
    print 'First target: ' + str(targetObject)
    
    while not rospy.is_shutdown():

        if targetReached:
            targetObj = getNext()            
            print 'Next target: ' + str(targetObject)
            targetReached = False

        twist = movement()

        pub.publish(twist)
        
        rospy.sleep(.01)

def getNext():
    global i
    global seekorder
    index = i
    i+=1

    if i is len(seekorder):
        i = 0
    nextObj = targetArray[index]
    return nextObj


if __name__ == '__main__':
    try:
        hello_create()
    except rospy.ROSInterruptException: pass
